#include "MagneticSensorI2C.h"

/**
 * 典型的12位AMS AS5600磁传感器通过I2C接口的配置
 *
 * 该结构体定义了AS5600磁传感器在I2C通信中的基本参数，包括芯片地址、位分辨率、角度寄存器地址以及数据起始位。
 */
MagneticSensorI2CConfig_s AS5600_I2C = {
        .chip_address = 0x36, // AS5600芯片的I2C地址，固定为0x36
        .bit_resolution = 12, // AS5600传感器的角度数据分辨率，为12位
        .angle_register = 0x0C, // AS5600中存储角度数据的寄存器地址
        .data_start_bit = 11 // AS5600角度数据的最高有效位（MSB）在寄存器中的起始位位置
};

MagneticSensorI2CConfig_s MT6701_I2C = {
        .chip_address = 0x0C,           // I2C地址（7位地址0x06左移1位，实际通信地址为0x0C）
        .bit_resolution = 14,           // 14位分辨率（0-16383对应0-360°）
        .angle_register = 0x03,         // 角度寄存器起始地址（高8位：0x03，低6位：0x04）
        .data_start_bit = 13            // 角度数据的最高有效位（MSB）在寄存器中的位置
};

/**
 * 典型的12位AMS AS5048磁传感器通过I2C接口的配置
 *
 * 该结构体定义了AS5048磁传感器在I2C通信中的基本参数，包括芯片地址、位分辨率、角度寄存器地址以及数据起始位。
 * 注意：AS5048的芯片地址可以通过引脚A1和A2进行配置，此处的0x40是其中一个可能的地址值。
 */
MagneticSensorI2CConfig_s AS5048_I2C = {
        .chip_address = 0x40,  // AS5048芯片的I2C地址，可通过引脚A1和A2配置。如果A1和A2都接地，该地址是可能的值
        .bit_resolution = 14, // AS5048传感器的角度数据分辨率，为14位
        .angle_register = 0xFE, // AS5048中存储角度数据的寄存器地址
        .data_start_bit = 15 // AS5048角度数据的最高有效位（MSB）在寄存器中的起始位位置
};

// MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb)
//  @param _chip_address  I2C chip address
//  @param _bit_resolution  bit resolution of the sensor
//  @param _angle_register_msb  angle read register
//  @param _bits_used_msb number of used bits in msb
MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb,
                                     int _bits_used_msb) {
    // chip I2C address
    chip_address = _chip_address;
    // angle read register of the magnetic sensor
    angle_register_msb = _angle_register_msb;
    // register maximum value (counts per revolution)
    cpr = _powtwo(_bit_resolution);

    // depending on the sensor architecture there are different combinations of
    // LSB and MSB register used bits
    // AS5600 uses 0..7 LSB and 8..11 MSB
    // AS5048 uses 0..5 LSB and 6..13 MSB
    // used bits in LSB
    lsb_used = _bit_resolution - _bits_used_msb;
    // extraction masks
    lsb_mask = (uint8_t) ((2 << lsb_used) - 1);
    msb_mask = (uint8_t) ((2 << _bits_used_msb) - 1);
    wire = &Wire;
}

MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config) {
    chip_address = config.chip_address;

    // angle read register of the magnetic sensor
    angle_register_msb = config.angle_register;
    // register maximum value (counts per revolution)
    cpr = _powtwo(config.bit_resolution);

    int bits_used_msb = config.data_start_bit - 7;
    lsb_used = config.bit_resolution - bits_used_msb;
    // extraction masks
    lsb_mask = (uint8_t) ((2 << lsb_used) - 1);
    msb_mask = (uint8_t) ((2 << bits_used_msb) - 1);
    wire = &Wire;
}

void MagneticSensorI2C::init(TwoWire *_wire) {

    wire = _wire;

    // I2C communication begin
    wire->begin();

    this->Sensor::init(); // call base class init
}

//  Shaft angle calculation
//  angle is in radians [rad]
float MagneticSensorI2C::getSensorAngle() {
    // (number of full rotations)*2PI + current sensor angle
    return (getRawCount() / (float) cpr) * _2PI;
}


// function reading the raw counter of the magnetic sensor
int MagneticSensorI2C::getRawCount() {
    return (int) MagneticSensorI2C::read(angle_register_msb);
}

// I2C functions
/*
* Read a register from the sensor
* Takes the address of the register as a uint8_t
* Returns the value of the register
*/
int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
    // read the angle register first MSB then LSB
    byte readArray[2];
    uint16_t readValue = 0;
    // notify the device that is aboout to be read
    wire->beginTransmission(chip_address);
    wire->write(angle_reg_msb);
    currWireError = wire->endTransmission(false);

    // read the data msb and lsb
    wire->requestFrom(chip_address, (uint8_t) 2);
    for (byte i = 0; i < 2; i++) {
        readArray[i] = wire->read();
    }

    // depending on the sensor architecture there are different combinations of
    // LSB and MSB register used bits
    // AS5600 uses 0..7 LSB and 8..11 MSB
    // AS5048 uses 0..5 LSB and 6..13 MSB
    readValue = (readArray[1] & lsb_mask);
    readValue += ((readArray[0] & msb_mask) << lsb_used);
    return readValue;
}

/*
* Checks whether other devices have locked the bus. Can clear SDA locks.
* This should be called before sensor.init() on devices that suffer i2c slaves locking sda
* e.g some stm32 boards with AS5600 chips
* Takes the sda_pin and scl_pin
* Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW
*/
int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) {

    pinMode(scl_pin, INPUT_PULLUP);
    pinMode(sda_pin, INPUT_PULLUP);
    delay(250);

    if (digitalRead(scl_pin) == LOW) {
        // Someone else has claimed master!");
        return 1;
    }

    if (digitalRead(sda_pin) == LOW) {
        // slave is communicating and awaiting clocks, we are blocked
        pinMode(scl_pin, OUTPUT);
        for (byte i = 0; i < 16; i++) {
            // toggle clock for 2 bytes of data
            digitalWrite(scl_pin, LOW);
            delayMicroseconds(20);
            digitalWrite(scl_pin, HIGH);
            delayMicroseconds(20);
        }
        pinMode(sda_pin, INPUT);
        delayMicroseconds(20);
        if (digitalRead(sda_pin) == LOW) {
            // SDA still blocked
            return 2;
        }
        _delay(1000);
    }
    // SDA is clear (HIGH)
    pinMode(sda_pin, INPUT);
    pinMode(scl_pin, INPUT);

    return 0;
}
